#include "core/main.h"

Graph::Graph()
{
    
}

Graph::Graph(const vector<GraphEdge> &edges)
{
    LoadEdges(edges);
}

void Graph::LoadEdges(const vector<GraphEdge> &edges)
{
    _edges = edges;
    GenerateAdjacencyList();
}

UINT Graph::HighestVertexIndex()
{
    UINT result = 0;
    for(UINT edgeIndex = 0; edgeIndex < _edges.size(); edgeIndex++)
    {
        const GraphEdge &curEdge = _edges[edgeIndex];
        result = Math::Max(result, curEdge.v[0]);
        result = Math::Max(result, curEdge.v[1]);
    }
    return result;
}

void Graph::GenerateAdjacencyList()
{
    UINT VertexCount = HighestVertexIndex() + 1;
    _vertices.resize(VertexCount);
    for(UINT EdgeIndex = 0; EdgeIndex < _edges.size(); EdgeIndex++)
    {
        const GraphEdge &curEdge = _edges[EdgeIndex];
        _vertices[curEdge.v[0]].edges.push_back(EdgeIndex);
        _vertices[curEdge.v[1]].edges.push_back(EdgeIndex);
    }
    _cachedVertex = -1;
}

struct DijkstraQueueElement
{
    DijkstraQueueElement() {}
    DijkstraQueueElement(UINT _VertexIndex, float _Value)
    {
        VertexIndex = _VertexIndex;
        Value = _Value;
    }
    UINT VertexIndex;
    float Value;
};

bool operator < (const DijkstraQueueElement &A, const DijkstraQueueElement &B)
{
    return A.Value > B.Value;
}

vector<UINT> Graph::ShortestPath(UINT endVertex, UINT startVertex)
{
    vector<UINT> pathEdgeIndices;

    //
    // startVertex & endVertex are flipped intentionally
    //
    if(startVertex == endVertex || endVertex >= _vertices.size() || startVertex >= _vertices.size())
    {
        return pathEdgeIndices;
    }

    if(_cachedVertex != startVertex)
    {
        _cachedVertex = startVertex;

        for(UINT VertexIndex = 0; VertexIndex < _vertices.size(); VertexIndex++)
        {
            GraphVertex &curVertex = _vertices[VertexIndex];
            curVertex.dist = 1e10f;
            curVertex.predecessorEdgeIndex = -1;
            curVertex.visited = false;
        }

        GraphVertex &sourceVertex = _vertices[startVertex];

        sourceVertex.dist = 0.0f;

        priority_queue<DijkstraQueueElement> queue;
        queue.push(DijkstraQueueElement(startVertex, 0.0f));
    
        while(!queue.empty())
        {
            DijkstraQueueElement QueueTop = queue.top();
            queue.pop();

            GraphVertex &curVertex = _vertices[QueueTop.VertexIndex];
            if(!curVertex.visited)
            {
                curVertex.visited = true;
                for(UINT AdjacentEdgeIndex = 0; AdjacentEdgeIndex < curVertex.edges.size(); AdjacentEdgeIndex++)
                {
                    GraphEdge &curEdge = _edges[curVertex.edges[AdjacentEdgeIndex]];
                    UINT otherVertexIndex = curEdge.GetOtherVertex(QueueTop.VertexIndex);
                    GraphVertex &otherVertex = _vertices[otherVertexIndex];
                    float NewDist = curVertex.dist + curEdge.weight;
                    if(NewDist < otherVertex.dist)
                    {
                        otherVertex.dist = NewDist;
                        otherVertex.predecessorEdgeIndex = curVertex.edges[AdjacentEdgeIndex];
                        queue.push(DijkstraQueueElement(otherVertexIndex, otherVertex.dist));
                    }
                }
            }
        }
    }

    if(_vertices[endVertex].predecessorEdgeIndex == -1)
    {
        
    }
    else
    {
        bool done = false;
        UINT curVertex = endVertex;
        while(!done)
        {
            UINT PredecessorEdgeIndex = _vertices[curVertex].predecessorEdgeIndex;
            if(PredecessorEdgeIndex == -1)
            {
                done = true;
            }
            else
            {
                pathEdgeIndices.push_back(PredecessorEdgeIndex);
                curVertex = _edges[PredecessorEdgeIndex].GetOtherVertex(curVertex);
            }
        }
    }

    return pathEdgeIndices;
}

/*void Graph::LoadFromEdgeMesh(const EdgeMesh &M)
{
    vector<GraphEdge> edges(M.FullEdges().size());
    for(UINT EdgeIndex = 0; EdgeIndex < edges.size(); EdgeIndex++)
    {
        GraphEdge &curEdge = edges[EdgeIndex];
        FullEdge &curMeshEdge = *(M.FullEdges()[EdgeIndex]);
        curEdge.v[0] = curMeshEdge.GetVertex(0).Index();
        curEdge.v[1] = curMeshEdge.GetVertex(1).Index();
        curEdge.weight = Vec3::Dist(curMeshEdge.GetVertex(0).Pos(), curMeshEdge.GetVertex(1).Pos());
    }
    LoadEdges(edges);
}*/
